#include <QPainter>
#include "handlersensor.h"

handlerSensor::handlerSensor(QObject *parent,ArClientBase *client) :
    QObject(parent),
    getSensorListCB(this,&handlerSensor::getSensorList),
    getSensorCurrentCB(this,&handlerSensor::getSensorCurrent)
{

    handledClient = client;


    if(!handledClient->dataExists("getSensorList"));
    else{
        handledClient->addHandler("getSensorList", &getSensorListCB);
        handledClient->addHandler("getSensorCurrent", &getSensorCurrentCB);
        handledClient->requestOnce("getSensorList");
    }

}


 void handlerSensor::getSensorList(ArNetPacket *packet){

     int numsensor;
     char nombre[512];
     ArNetPacket *pcommand;


     numsensor = packet->bufToByte2();

     printf("Numero de sensores: %d\n",numsensor);

     for(int x=0;x<numsensor;x++){
         packet->bufToStr(nombre,512);
         printf("Nombre sensor %d: %s\n",x+1,nombre);

         if(!strcmp(nombre,"sonar")){
             printf("Encontrado sonar\n");
             sonarpoints.begin();
             pcommand = new ArNetPacket();
             strcpy(sonarname,nombre);
             pcommand->strToBuf(nombre);
             handledClient->request("getSensorCurrent",200,pcommand);

         }

         if(QString(nombre).contains("lms",Qt::CaseInsensitive)){
             printf("Encontrado laser\n");
             laserpoints.begin();
             pcommand = new ArNetPacket();
             strcpy(lasername,nombre);
             pcommand->strToBuf(nombre);
             handledClient->request("getSensorCurrent",200,pcommand);

         }

     }

 }


 void handlerSensor::getSensorCurrent(ArNetPacket *packet){

     int numreadings;
     char nombre[512];
     int X,Y;
     QGraphicsEllipseItem *point;
     ArNetPacket *paquete = new ArNetPacket();

     numreadings = packet->bufToByte2();
     packet->bufToStr(nombre,512);


     if(!strcmp(nombre,sonarname)){

         for(int x=0;x<sonarpoints.size();x++){
             emit signal_remove_ellipse(sonarpoints[x]);
         }



         if(numreadings>=0)sonarpoints.resize(numreadings);



         for(int x=0;x<numreadings;x++){

             X = packet->bufToByte4();
             Y = packet->bufToByte4();

             point = new QGraphicsEllipseItem(X-25,Y*(-1)-25,50,50,0,0);

             point->setPen(QPen(Qt::black));
             point->setBrush(QBrush(Qt::green));

             if(x<sonarpoints.size())sonarpoints[x] = point;
             emit signal_paint_ellipse(point);

         }

         if(strlen(lasername)){
             paquete->strToBuf(lasername);
             handledClient->request("getSensorCurrent",200,paquete);
         }else{
             paquete->strToBuf(sonarname);
             handledClient->request("getSensorCurrent",200,paquete);
         }
     }

     if(!strcmp(nombre,lasername)){

         for(int x=0;x<laserpoints.size();x++){
             emit signal_remove_ellipse(laserpoints[x]);
         }



         if(numreadings>=0)laserpoints.resize(numreadings);


         for(int x=0;x<numreadings;x++){

             X = packet->bufToByte4();
             Y = packet->bufToByte4();

             point = new QGraphicsEllipseItem(X-25,Y*(-1)-25,50,50,0,0);

             point->setPen(QPen(Qt::black));
             point->setBrush(QBrush(Qt::red));

             if(x<laserpoints.size())laserpoints[x] = point;
             emit signal_paint_ellipse(point);
         }

        if(strlen(sonarname)){
             paquete->strToBuf(sonarname);
             handledClient->request("getSensorCurrent",200,paquete);
         }else{
             paquete->strToBuf(lasername);
             handledClient->request("getSensorCurrent",200,paquete);
         }

        }

 }
